//author wssccc
#define USE_ARCTURN

#define MAX_DC 100
#define PULSE_TIME 100 //us
#define TURN_SPEED 10

#define LEFT_MOTION_FORWARD 0b01000000
#define LEFT_MOTION_BACKWARD 0b10000000
#define RIGHT_MOTION_FORWARD 0b00100000
#define RIGHT_MOTION_BACKWARD 0b00010000

#define ARC_FACTOR 1   //test

void left_arcTurn_begin(unsigned char leftDC){
	unsigned int i;
	unsigned char j;
	unsigned char scale;
	unsigned char rightDC;
	unsigned char motion1;
	unsigned char motion2;
	rightDC=MAX_DC-leftDC;
	if(leftDC>rightDC){
		scale=MAX_DC/rightDC;
		motion1=LEFT_MOTION_FORWARD;
		motion2=RIGHT_MOTION_BACKWARD;
	}else{
		scale=MAX_DC/leftDC;
		motion1=RIGHT_MOTION_BACKWARD;
		motion2=LEFT_MOTION_FORWARD;
	}
	
	for(i=0;i<ARC_FACTOR;++i){
		for(j=0;j<MAX_DC;++j){
			PORTD=motion1;
			delay_us(PULSE_TIME*TURN_SPEED);
			if(j%scale==0){
				PORTD=motion2;
				delay_us(PULSE_TIME*TURN_SPEED);
			}
		}
	}
}


void arcTurn_begin(unsigned char leftDC){
	unsigned int i;
	unsigned char j;
	unsigned char scale;
	unsigned char rightDC;
	unsigned char motion1;
	unsigned char motion2;
	rightDC=MAX_DC-leftDC;
	if(leftDC>rightDC){
		scale=MAX_DC/rightDC;
		motion1=LEFT_MOTION_FORWARD;
		motion2=RIGHT_MOTION_FORWARD;
	}else{
		scale=MAX_DC/leftDC;
		motion1=RIGHT_MOTION_FORWARD;
		motion2=LEFT_MOTION_FORWARD;
	}
	
	for(i=0;i<ARC_FACTOR;++i){
		for(j=0;j<MAX_DC;++j){
			PORTD=motion1;
			delay_us(PULSE_TIME*TURN_SPEED);
			if(j%scale==0){
				PORTD=motion2;
				delay_us(PULSE_TIME*TURN_SPEED);
			}
		}
	}
}

void arcTurn_end(){
	 PORTD=0x00;
}


void turnLeft(unsigned int speed){
	arcTurn_begin(R18);
	arcTurn_end();
	
	}
	
void turnRight(unsigned int speed){
	 arcTurn_begin(MAX_DC-R18);
	 arcTurn_end();
	 }
	  